﻿using ROSBridgeLib;
using ROSBridgeLib.sensor_msgs;
using ROSBridgeLib.std_msgs;
using ROSBridgeLib.geometry_msgs;
using ROSBridgeLib.nav_msgs;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using SimpleJSON;


public class RobotposeSubscriber : BaseSubscriber {




    private Vector3 targetPosition;

    private Quaternion targetRotation;

    void Start()
    {

        topic = "robot_pose";

        messageType = "geometry_msgs/PoseStamped";
      
    }



    public override void OnReset()
    {
        base.OnReset();

        targetPosition=Vector3.zero;

        targetRotation=Quaternion.identity;


    }

    public override ROSBridgeMsg Parse(JSONNode msg)
    {
        return new PoseStampedMsg(msg["msg"]);

    }

    public override void OnMessage()
    {

       
        PoseStampedMsg postStamp=(PoseStampedMsg)GetTopParsedPackage();

        if (postStamp==null)
        {
            return;

        }

       
        targetPosition=postStamp.pose.position.ToVector3();

        targetRotation=postStamp.pose.orientation.ToQuaternion();

    }


    void Update()
    {

        transform.localPosition=Vector3.Lerp(transform.localPosition,targetPosition,Time.deltaTime);


        transform.localRotation=Quaternion.Lerp(transform.localRotation,targetRotation,Time.deltaTime);

//
//        transform.localPosition=targetPosition;
//
//        transform.localRotation=targetRotation;
//
//


    }
}
